#include "../cominc/algoLinearCalib.h"
#include "../cominc/cli.h"
#include <string.h>
#include <stdlib.h>
#include <math.h>

#ifdef USING_CLIB_CLI
/* 设置参数表 */
void algoLinearCalib_SetParaTable(algoLinearCalibParaTable *src, int paraId)
{
    LOG("algoLinearCalibParaTable paraId = %d set. (%d)", paraId, sizeof(algoLinearCalibParaTable));
    memcpy((uint8_t *)(talbeList + paraId), (uint8_t *)src, sizeof(algoLinearCalibParaTable));

    algoLinearCalib_PrintParaTable(NULL, 0);
}

/* 获取参数表 */
algoLinearCalibParaTable *algoLinearCalib_GetParaTable(void)
{
    return talbeList;
}

/* 控制台打印参数表 */
void algoLinearCalib_PrintParaTable(char *cmd, int len)
{
    for (int cnt = 0; cnt < ALGO_LINEAR_CALIB_PARA_TABLE_ID_MAX; cnt++)
    {
        if (talbeList[cnt].tValid != 1)
            continue;
        LOG("ParaTable[%02d] : {1, %d, ", cnt, talbeList[cnt].validPtsCnt);
        for (int i = 0; i < ALGO_LINEAR_CALIB_PARA_TABLE_PTS_MAX; i++)
        {
            LOG("{%.2f, %.2f}, ", talbeList[cnt].pts[i].x, talbeList[cnt].pts[i].y);
        }
        LOG("}\r\n");
    }
}
/* 控制台解析参数表 */
void algoLinearCalib_ModifyParaTable(char *cmd, int len)
{
#ifdef USING_CLIB_CLI
    if (cmd == NULL || len == 0)
    {
        LOG("[ERROR] cmd == NULL || len ==0");
        return;
    }
    int id, tValid, validCnt;
    char *body = (char *)malloc(len);
    memset(body, 0, len);
    /* 填充参数表控制字 */
    sscanf(cmd, "ParaTable[%d]:{%d,%d,%s}", &id, &tValid, &validCnt, body);
    if (id >= ALGO_LINEAR_CALIB_PARA_TABLE_ID_MAX)
    {
        LOG("[ERROR] id(% d) > ALGO_LINEAR_CALIB_PARA_TABLE_ID_MAX(% d)", id, ALGO_LINEAR_CALIB_PARA_TABLE_ID_MAX);
        goto end;
    }
    if ((tValid != 0) && (tValid != 1))
    {
        LOG("[ERROR] tValid(%d) must equal to 1 or 0", tValid);
        goto end;
    }
    if (validCnt >= ALGO_LINEAR_CALIB_PARA_TABLE_PTS_MAX)
    {
        LOG("[ERROR] validCnt(%d) >= ALGO_LINEAR_CALIB_PARA_TABLE_PTS_MAX(%d)", validCnt, algoLinearCalib_ModifyParaTable);
        goto end;
    }
    talbeList[id].tValid = tValid;
    talbeList[id].validPtsCnt = validCnt;
    /* 解析各个修正点 */
    char *pBody = body;
    float x, y, n = 3;
    for (int i = 0; i < ALGO_LINEAR_CALIB_PARA_TABLE_PTS_MAX && n == 3; i++)
    {
        n = sscanf(pBody, "{%f, %f},%s", &x, &y, pBody);
        if (n != 3)
        {
            LOG("[ERROR] sytax error, n = %d, body = %s", n, body);
            goto end;
        }
        talbeList[id].pts[i].x = x;
        talbeList[id].pts[i].y = y;
    }
end:
    free(body);
    /* 显示参数表 */
    algoLinearCalib_PrintParaTable(NULL, 0);
#endif
}
#endif
/* 内部依赖方法 */
static void sortCurve(algoLinearCalib *caliber);
static ERRCODE isAlgoLinearCalibValid(const uint8_t* mem, size_t memLen);
static void RecalcCRC(algoLinearCalib *algo);

void algoLinearCalib_Init(algoLinearCalib* algo, uint8_t* mem, size_t memLen)
{
    if (algo == NULL) {
        return;
    }
    if(isAlgoLinearCalibValid(mem, memLen) == ERR_SUCCESS) {
        memcpy(algo, mem, memLen);
        LOG("algoLinearCalib Index[%d] Load From Memory[%08x].", mem, algo->tableID);
    } else {
        memset(algo, 0, sizeof (algoLinearCalib));
        LOG("algoLinearCalib Index[%d] Fresh New.");
    }
}

static void sortCurve(algoLinearCalib *caliber)
{
    if (caliber == NULL) {
        return;
    }
    dataPt* p = caliber->pts;
    for (uint32_t idx1 = 0; idx1 < ALGO_LINEAR_CALIB_PTSCNT - 1; idx1++) {
        for (uint32_t idx2 = 0; idx2 < ALGO_LINEAR_CALIB_PTSCNT - 1 - idx1; idx2++) {
            if (p[idx2].x > p[idx2 + 1].x) {
                dataPt temp = p[idx2];
                p[idx2] = p[idx2 + 1];
                p[idx2 + 1] = temp;
            }
        }
    }
}

void algoLinearCalib_InsertPoint(algoLinearCalib *caliber, dataPt point)
{
    if (caliber == NULL) {
        return;
    }
    caliber->pts[(ALGO_LINEAR_CALIB_PTSCNT - 1) / 2] = point;
    sortCurve(caliber);
    RecalcCRC(caliber);
}

void algoLinearCalib_SetRange(algoLinearCalib *caliber, dataPt min, dataPt max)
{
    if (caliber == NULL) {
        return;
    }
    caliber->pts[0] = min;
    caliber->pts[ALGO_LINEAR_CALIB_PTSCNT - 1] = max;
    sortCurve(caliber);
    RecalcCRC(caliber);
}

/* 获取修正值 */
float algoLinearCalib_Calc(const algoLinearCalib *caliber, float val)
{
    if (caliber == NULL) {
        return 0.0f;
    }
    double xVal = val * pow(10, caliber->xUnit);
    double yResult = val;
    for (int cnt = 0; cnt < ALGO_LINEAR_CALIB_PTSCNT - 1; cnt++) {
        dataPt pt1 = caliber->pts[cnt];
        dataPt pt2 = caliber->pts[cnt + 1];
        double x1 = pt1.x * pow(10, caliber->xUnit); // 不能化为定点数再运算
        double x2 = pt2.x * pow(10, caliber->xUnit);
        double y1 = pt1.y * pow(10, caliber->yUnit);
        double y2 = pt2.y * pow(10, caliber->yUnit);
        if (x1 <= xVal && xVal < x2) { // 要求校准曲线单调递增
            yResult = y1 + (y2 - y1) / (x2 - x1) * (xVal - x1);
        }
    }
    return (float)yResult;
}

static ERRCODE isAlgoLinearCalibValid(const uint8_t* mem, size_t memLen)
{
    if (mem == NULL || memLen != sizeof(algoLinearCalib)) {
        return ERR_ASSERT;
    }
    uint32_t crcCheck = 0;
    for (size_t idx = 0; idx < memLen - 4; idx++) {
        crcCheck += mem[idx];
    }
    return crcCheck == ((algoLinearCalib*)mem)->crc ? ERR_SUCCESS : ERR_FRMEERR;
}

static void RecalcCRC(algoLinearCalib *algo)
{
    if (algo == NULL) {
        return;
    }
    uint32_t crcCheck = 0;
    for (size_t idx = 0; idx < sizeof (algoLinearCalib) - 4; idx++) {
        crcCheck += ((uint8_t*)algo)[idx];
    }
    algo->crc = crcCheck;
}